#include "TangentField.h"

TangentField::~TangentField()
{
}

Vector2 TangentField::get_trajectory(Vector2 & tank_loc) {
	Vector2 trajectory;
	int next;
	double dist=0;
	double dist2=0;
	double scale;
	Vector2 start, end, intersect;
	Vector2 start2, end2;
	for( int i = 0; i < _obstacle->size; i ++ ){
		next = i + 1;
		if(i == _obstacle->size -1){
			next = 0;
		}
		start.x = _obstacle->o_corner[i][0], start.y = _obstacle->o_corner[i][1]; 
		end.x = _obstacle->o_corner[next][0], end.y = _obstacle->o_corner[next][1]; 
		
		if(next == _obstacle->size-1){
			start2.x = _obstacle->o_corner[0][0], start2.y = _obstacle->o_corner[0][1]; 
			end2.x = _obstacle->o_corner[1][0], end2.y = _obstacle->o_corner[1][1];
		}
		else{
			start2.x = _obstacle->o_corner[next][0], start2.y = _obstacle->o_corner[next][1]; 
			end2.x = _obstacle->o_corner[next+1][0], end2.y = _obstacle->o_corner[next+1][1]; 
		}
		if(lineSegmentIntersect(_loc, tank_loc, start, end, intersect)){
			dist = fabs(GetDist(start, end, tank_loc));
			//dist2 = fabs(GetDist(start2, end2, tank_loc));
			//if(dist < dist2) dist = dist2;

			dist2 = (tank_loc-_loc).magnitude();
			if(dist2 <= maxRadius){
				if(dist > maxDist) scale = 0;
				else if(dist < minDist) scale = 1;
				else scale = 1.0/(1.0 + (dist - minDist));
			}
			else
				scale = 0.0;
			trajectory += (end - start).normalize() * scale;
			
		}
	}
	
	return trajectory;
}

bool PrivateLineToLineIntersection(Vector2 vertex1, Vector2 vertex2,
                  Vector2 vertex3, Vector2 vertex4, float & r, float & s)
{
    float d;
    //Make sure the lines aren't parallel
    Vector2 vertex1to2 = vertex2 - vertex1;
    Vector2 vertex3to4 = vertex4 - vertex3;
    if(vertex1to2.y / vertex1to2.x != vertex3to4.y / vertex3to4.x)
    {
       d = vertex1to2.x * vertex3to4.y - vertex1to2.y * vertex3to4.x;
       if (d != 0)
       {
          Vector2 vertex3to1 = vertex1 - vertex3;
          r = (vertex3to1.y * vertex3to4.x - vertex3to1.x * vertex3to4.y) / d;
          s = (vertex3to1.y * vertex1to2.x - vertex3to1.x * vertex1to2.y) / d;
          return true;
       }
    }
    return false;
}

bool TangentField::lineSegmentIntersect(Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2, Vector2& intersect)
{
	float r, s;
    if(PrivateLineToLineIntersection(start1, end1, start2, end2, r, s))
    {
       if (r >= 0)
       {
          if (s >= 0 && s <= 1)
          {
             intersect = start1 + (end1 - start1) * r;
			 return true;
          }
       }
    }
    return false;
}

double TangentField::GetDist(Vector2 start, Vector2 end, Vector2 point){
	Vector2 n = (start - end).normalize();
	
	double temp = n.x;
	n.x = -n.y;
	n.y = temp;

	double c = -n.x*start.x - n.y*start.y;

	return n.x*point.x + n.y*point.y + c;
}